#include <algorithm/analytical_model/algorithm/cpu/GeometryUtils.h>
#include <algorithm/analytical_model/algorithm/cpu/VectorConvert.h>
#include <algorithm/analytical_model/algorithm/geometry/Utils.h>

using namespace solar::cpu;
using namespace solar;
using namespace solar::cuda::inter_op;

auto solar::cpu::localToWorldRotate(const Float3& d_local, const Float3& aligned_normal) -> Float3
{
    Vec3 n = toVec3(aligned_normal);
    Vec3 u = cross({0, 1, 0}, n).get_unit_vector();
    Vec3 v = cross(u, n).get_unit_vector();
    auto vec1 = u * d_local.x;
    auto vec2 = n * d_local.y;
    auto vec3 = v * d_local.z;
    return toFloat3(vec1 + vec2 + vec3);
}

auto solar::cpu::translate(const Float3& d_in, const Float3& translate_vector) -> Float3
{
    return toFloat3(toVec3(d_in) + toVec3(translate_vector));
}

auto solar::cpu::rotateY(const Float3& origin, const Float3& old_dir,
                         const Float3& new_dir) -> Float3
{
    auto old_dir_vec = toVec3(old_dir);
    auto new_dir_vec = toVec3(new_dir);
    // when parallel to (0,1,0), sin = 0, no effect
    auto dir = (solar::cross(old_dir_vec, new_dir_vec).y() > 0) ? 1 : -1;
    auto cos_value = solar::dot(old_dir_vec, new_dir_vec);
    auto sin_value = dir * std::sqrt(1 - cos_value * cos_value);
    auto vec1 = Vec3(cos_value, 0.5, cos_value);
    auto vec2 = Vec3(-sin_value, 0.5, sin_value);
    vec2 = vcl::permute4<2, 1, 0, -1>(vec2.get_vec());
    auto origin_vec = toVec3(origin);
    return toFloat3(vec1 * origin_vec + vec2 * origin_vec);
}

auto solar::cpu::refractRay(const Float3& dir1, const Float3& normal, float refract, float theta_1,
                            float theta_2) -> Float3
{
    auto dir1_vec = toVec3(dir1);
    auto normal_vec = toVec3(normal);
    return toFloat3(dir1_vec / refract +
                    normal_vec * (std::cos(theta_1) / refract - std::cos(theta_2)));
}

auto solar::cpu::rayParallelogramIntersect(const Float3& orig, const Float3& dir, const Float3& A,
                                           const Float3& B,
                                           const Float3& C) -> std::tuple<float, float, float, bool>
{
    float t = NAN;
    float u = NAN;
    float v = NAN;
    auto orig_vec = toVec3(orig);
    auto dir_vec = toVec3(dir);
    auto A_vec = toVec3(A);
    auto B_vec = toVec3(B);
    auto C_vec = toVec3(C);
    auto AB = B_vec - A_vec;
    auto AC = C_vec - A_vec;
    // vector p
    auto p = solar::cross(dir_vec, AC);
    // determinant
    auto det = solar::dot(AB, p);

    // if determinant is close to 0, ray lies in plane of parallelogram
    if (std::abs(det) < kEpsilon)
    {
        return {t, u, v, false};
    }

    auto T = orig_vec - A_vec;

    // Calculate u and make sure u <= 1
    u = solar::dot(T, p) / det;
    if (u < 0 || u > 1)
    {
        return {t, u, v, false};
    }

    auto q = solar::cross(T, AB);

    // Calculate v and make sure v <= 1
    v = solar::dot(dir_vec, q) / det;
    if (v < 0 || v > 1)
    {
        return {t, u, v, false};
    }

    t = solar::dot(AC, q) / det;
    return {t, u, v, t >= kEpsilon};
}

auto solar::cpu::intersect(const Float3& normal, const Float3& plane_origin, const Float3& vertex,
                           const Float3& direction) -> std::tuple<Float3, double>
{
    auto result =
        solar::intersect(toVec3(normal), toVec3(plane_origin), toVec3(vertex), toVec3(direction));
    return {toFloat3(std::get<0>(result)), std::get<1>(result)};
}
